#include "lio.h"

#include <chrono>
#include <iostream>

#include <algorithm>

// #include <omp.h>

#include <pcl/filters/conditional_removal.h>
#include <pcl/filters/voxel_grid.h>

namespace slam
{

    /** constructor of lio
     *
     * @param ivox the map data structure
     * @return None
     */
    Lio::Lio(std::shared_ptr<IVoxType> &ivox, const Eigen::Matrix4d &initial_transform) : /* iVox data structure for mapping */
                                                                                          ivox_{ivox},

                                                                                          /* */
                                                                                          lio_state_{lio_state_t::INITIALIZING},

                                                                                          /* number of imu data accumulation when lio initializing,
                                                                                             the imu bias and covariance will be calculated based on the data. */
                                                                                          minimum_initial_imu_data_{20},

                                                                                          /* maximum ikfom iterations for every lidar frame */
                                                                                          ikfom_max_iterations_{4},

                                                                                          /* covariance of noise of gyro, acceleration,
                                                                                             bias of gyro and bias of acceleration */
                                                                                          noise_covariance{process_noise_cov()},

                                                                                          /* initial translation form lidar to imu */
                                                                                          initial_lidar_translation_wrt_imu_{Eigen::Vector3d::Zero()},

                                                                                          /* initial rotation from lidar to imu */
                                                                                          initial_lidar_rotation_wrt_imu_{Eigen::Matrix3d::Identity()},

                                                                                          /* lower limit of valid range in meters,
                                                                                             pointcloud within this range will be filtered */
                                                                                          range_lower_limit_{8},

                                                                                          /* upper limit of valid range in meters,
                                                                                             pointcloud out of this range will be filtered */
                                                                                          range_upper_limit_{45},

                                                                                          /* downsampling size for voxel filter */
                                                                                          downsampling_size_{4},

                                                                                          /* current frame of lidar data that the lio is processing */
                                                                                          current_lidar_data_{0, nullptr},

                                                                                          /* lidar measurement covariance */
                                                                                          lidar_covariance_{0.001},

                                                                                          /* maximum nearest points number for knn */
                                                                                          nearest_points_number_{5},

                                                                                          /* minimum nearest points need to be found */
                                                                                          minimum_nearest_points_{3},

                                                                                          /* knn search range */
                                                                                          max_nearest_points_search_range_{1.0},

                                                                                          initial_transform{initial_transform},

#ifdef LIO_DEBUG
                                                                                          /* for debugging: total lidar frames */
                                                                                          frames_{0},
                                                                                          /* for debugging: average running time for lidar frames */
                                                                                          avg_time_{0},
                                                                                          /* for debugging: maximum running time for lidar frames */
                                                                                          max_time_{0},
#endif /* LIO_DEBUG */

                                                                                          lidar_time_{0.0},

                                                                                          imu_time_{0.0},

                                                                                          lidar_updated_{0},

                                                                                          /* the observation function offers H matrix and dH/dx matrix to ikfom */
                                                                                          observation_function_{
                                                                                              [this](state_ikfom &s, esekfom::dyn_share_datastruct<double> &ekfom_data)
                                                                                              { ObservationModel_(s, ekfom_data); }}
    {

#ifdef LIO_DEBUG
        /* set cout float format */
        std::cout.setf(std::ios_base::fixed, std::ios_base::floatfield);
#endif /* LIO_DEBUG */
    }

    Lio::Lio(std::shared_ptr<IVoxType> &ivox) : Lio::Lio(ivox, Eigen::Matrix4d::Identity())
    {
    }

    /** destructor of lio
     *
     * @param ivox the map data structure
     * @return None
     */
    Lio::~Lio()
    {
        /* nothing to destroy */
    }

    /** estimate plane based on nearest points
     *
     * @param pca_result parameters of the plane
     * @param points nearest points
     * @param threshold the maximum distance between nearest points and the plane
     * @return whether the plane estimated successfully,
     *         the distances between points and the plane are less than the threshold
     */
    bool Lio::EstimatePlane_(
        Eigen::Matrix<float, 4, 1> &pca_result,
        const IVoxType::PointVector &point,
        const float &threshold = 0.1f)
    {
        /* enough points needed to estimate the plane */
        if (point.size() < 3)
        {
            return false;
        }

        /* build equation Ax=b */
        Eigen::MatrixXf A(point.size(), 3);
        Eigen::VectorXf b(point.size(), 1);

        A.setZero();
        b.setOnes();
        b *= -1.0f;

        for (int j = 0; j < point.size(); j++)
        {
            A(j, 0) = point[j].x;
            A(j, 1) = point[j].y;
            A(j, 2) = point[j].z;
        }

        /* QR decomposition with Household method */
        Eigen::Vector3f normvec = A.colPivHouseholderQr().solve(b);

        float n = normvec.norm();
        pca_result(0) = normvec(0) / n;
        pca_result(1) = normvec(1) / n;
        pca_result(2) = normvec(2) / n;
        pca_result(3) = 1.0 / n;

        /* all nearest points in the threshold */
        for (const auto &p : point)
        {
            Eigen::Matrix<float, 4, 1> temp = p.getVector4fMap();
            temp[3] = 1.0;
            if (fabs(pca_result.dot(temp)) > threshold)
            {
                return false;
            }
        }
        return true;
    }

    /** calculate valid rate, check validity
     *
     * @param state_ikfom the current state of ikfom
     * @return valid rate
     */
    bool Lio::CheckValidity_(state_ikfom &s)
    {
        /* current lidar data */
        PointCloudType::Ptr &pointcloud = current_lidar_data_.pointcloud;

        /* rotation and translation from world to lidar */
        Eigen::Matrix3f rotation_world_lidar{(s.rot * s.offset_R_L_I).cast<float>()};
        Eigen::Vector3f translation_world_lidar{(s.rot * s.offset_T_L_I + s.pos).cast<float>()};

        /* nearest points, plane, distance between plane and point */
        // nearest_point_norm_residual_.resize(pointcloud->size());

        /* indicate points that should be add to the map */
        // points_add_to_map_.resize(pointcloud->size());

        /* points number valid for residual calculation */
        int valid_point_number = 0;
        /* points number invalid for residual calculation, need to add to map */
        int invalid_point_number = 0;

        // #   pragma omp parallel for
        for (auto point_iterator = pointcloud->points.begin();
             point_iterator < pointcloud->points.end();
             point_iterator++)
        {
            /* transform point from lidar to world */
            PointType point_world;
            point_world.getVector3fMap() =
                rotation_world_lidar * point_iterator->getVector3fMap() + translation_world_lidar;

            /* get nearest points */
            IVoxType::PointVector nearest_points;
            ivox_->GetClosestPoint(
                point_world,
                nearest_points,
                nearest_points_number_,
                max_nearest_points_search_range_);

            /* estimate plane, if failed, add points to map */
            Eigen::Vector4f plane;
            if (nearest_points.size() < minimum_nearest_points_ || !EstimatePlane_(plane, nearest_points))
            {
                // std::lock_guard<std::mutex> lock(mutex_);
                // points_add_to_map_[invalid_point_number] = point_iterator - pointcloud->points.begin();
                invalid_point_number++;
                continue;
            }

            /* calculate distance between point and plane */
            // Eigen::Vector4f point {point_world.getVector4fMap()};
            // point[3] = 1.0;
            // float distance = plane.dot(point);

            /* lock to avoid unatomic operation */
            // std::lock_guard<std::mutex> lock(mutex_);
            // nearest_point_norm_residual_[valid_point_number] =
            //     std::tuple<Eigen::Vector3f, Eigen::Vector4f, float>(
            //         point_iterator->getVector3fMap(),
            //         plane,
            //         distance);
            valid_point_number++;
        }

        valid_rate = double(valid_point_number) / double(pointcloud->size());
#ifdef LIO_DEBUG
        std::cout << "valid: " << valid_point_number << " (" << valid_rate << "), total: " << pointcloud->size() << std::endl;
#endif /* LIO_DEBUG */
        return valid_rate > 0.4;
    }

    /** the observation model offers H matrix and dH/dx matrix to ikfom
     *
     * @param state_ikfom the current state of ikfom
     * @param ekfom_data ikfom data structure to store H matrix and dH/dx matrix
     * @return None
     */
    void Lio::ObservationModel_(state_ikfom &s, esekfom::dyn_share_datastruct<double> &ekfom_data)
    {

#ifdef LIO_DEBUG
        auto t_start = std::chrono::system_clock::now();
#endif /* LIO_DEBUG */

        /* current lidar data */
        PointCloudType::Ptr &pointcloud = current_lidar_data_.pointcloud;

        /* rotation and translation from world to lidar */
        Eigen::Matrix3f rotation_world_lidar{(s.rot * s.offset_R_L_I).cast<float>()};
        Eigen::Vector3f translation_world_lidar{(s.rot * s.offset_T_L_I + s.pos).cast<float>()};

        /* nearest points, plane, distance between plane and point */
        nearest_point_norm_residual_.resize(pointcloud->size());
        /* indicate points that should be add to the map */
        points_add_to_map_.resize(pointcloud->size());

        /* points number valid for residual calculation */
        int valid_point_number = 0;
        /* points number invalid for residual calculation, need to add to map */
        int invalid_point_number = 0;

        // #   pragma omp parallel for
        for (auto point_iterator = pointcloud->points.begin();
             point_iterator < pointcloud->points.end();
             point_iterator++)
        {
            /* transform point from lidar to world */
            PointType point_world;
            point_world.getVector3fMap() =
                rotation_world_lidar * point_iterator->getVector3fMap() + translation_world_lidar;

            /* get nearest points */
            IVoxType::PointVector nearest_points;
            ivox_->GetClosestPoint(
                point_world,
                nearest_points,
                nearest_points_number_,
                max_nearest_points_search_range_);

            /* estimate plane, if failed, add points to map */
            Eigen::Vector4f plane;
            if (nearest_points.size() < minimum_nearest_points_ || !EstimatePlane_(plane, nearest_points))
            {
                std::lock_guard<std::mutex> lock(mutex_);
                points_add_to_map_[invalid_point_number] = point_iterator - pointcloud->points.begin();
                invalid_point_number++;
                continue;
            }

            /* calculate distance between point and plane */
            Eigen::Vector4f point{point_world.getVector4fMap()};
            point[3] = 1.0;
            float distance = plane.dot(point);

            /* lock to avoid unatomic operation */
            std::lock_guard<std::mutex> lock(mutex_);
            nearest_point_norm_residual_[valid_point_number] =
                std::tuple<Eigen::Vector3f, Eigen::Vector4f, float>(
                    point_iterator->getVector3fMap(),
                    plane,
                    distance);
            valid_point_number++;
        }

        /* return if no valid nearest points for all points */
        if (0 == valid_point_number)
        {
            ekfom_data.valid = false;
            return;
        }

        /* resize to actual used space */
        nearest_point_norm_residual_.resize(valid_point_number);
        points_add_to_map_.resize(invalid_point_number);

        ekfom_data.h_x = Eigen::MatrixXd::Zero(valid_point_number, 12);
        ekfom_data.h.resize(valid_point_number);

        /* transform */
        Eigen::Matrix3f rotation_lidar_imu{s.offset_R_L_I.toRotationMatrix().cast<float>()};
        Eigen::Vector3f translataion_lidar_imu{s.offset_T_L_I.cast<float>()};
        Eigen::Matrix3f rotation_world_imu{s.rot.toRotationMatrix().transpose().cast<float>()};

        /* H matrix and dH/dx matrix, see Kalman Filters on Differentiable Manifolds, 2021 */
        // #   pragma omp parallel for
        for (auto point_norm_residual_iterator = nearest_point_norm_residual_.begin();
             point_norm_residual_iterator < nearest_point_norm_residual_.end();
             point_norm_residual_iterator++)
        {
            Eigen::Vector3f point_lidar{std::get<0>(*point_norm_residual_iterator)};
            Eigen::Matrix3f point_lidar_cross{SKEW_SYM_MATRIX(point_lidar)};
            Eigen::Vector3f point_imu{rotation_lidar_imu * point_lidar + translataion_lidar_imu};
            Eigen::Matrix3f point_imu_cross{SKEW_SYM_MATRIX(point_imu)};

            Eigen::Vector3f norm{std::get<1>(*point_norm_residual_iterator).head<3>()};

            Eigen::Vector3f norm_imu{rotation_world_imu * norm};
            Eigen::Vector3f point_lidar_cross_rotation_imu_world_norm_imu{point_lidar_cross * rotation_lidar_imu.transpose() * norm_imu};
            Eigen::Vector3f point_imu_cross_norm_imu{point_imu_cross * norm_imu};

            auto index = point_norm_residual_iterator - nearest_point_norm_residual_.begin();
            ekfom_data.h_x.block<1, 12>(index, 0) << norm[0], norm[1], norm[2],
                point_imu_cross_norm_imu[0], point_imu_cross_norm_imu[1], point_imu_cross_norm_imu[2],
                point_lidar_cross_rotation_imu_world_norm_imu[0],
                point_lidar_cross_rotation_imu_world_norm_imu[1],
                point_lidar_cross_rotation_imu_world_norm_imu[2],
                norm_imu[0], norm_imu[1], norm_imu[2];

            ekfom_data.h(index) = -static_cast<double>(std::get<2>(*point_norm_residual_iterator));
        }
    }

    /** remove outlier points
     *
     * @param pointcloud pointcloud to be filtered
     * @return None
     */
    void Lio::RemoveOutlierPoints_(const PointCloudType::Ptr &pointcloud)
    {
        pcl::ConditionAnd<PointType>::Ptr range_cond(new pcl::ConditionAnd<PointType>);
        range_cond->addComparison(
            pcl::FieldComparison<PointType>::Ptr(
                new pcl::FieldComparison<PointType>("range", pcl::ComparisonOps::GT, range_lower_limit_ * 1000)));
        range_cond->addComparison(
            pcl::FieldComparison<PointType>::Ptr(
                new pcl::FieldComparison<PointType>("range", pcl::ComparisonOps::LT, range_upper_limit_ * 1000)));
        pcl::ConditionalRemoval<PointType> cond_filter;

        cond_filter.setCondition(range_cond);
        cond_filter.setInputCloud(pointcloud);
        cond_filter.setKeepOrganized(false);
        cond_filter.filter(*pointcloud);
    }

    /** initialize kalman filter, the lio should be static
     *
     * @return None
     */
    void Lio::InitializeKF_()
    {
        /* mean angular velocity and acceleration */
        Eigen::Vector3d mean_angular_velocity = Eigen::Vector3d::Zero();
        Eigen::Vector3d mean_acceleration = Eigen::Vector3d::Zero();

        imu_data_t imu_data;

        /* average value and covariance calculation */
        for (int i = 0; i < imu_data_buffer_.size(); i++)
        {
            imu_data = imu_data_buffer_.front();

            Eigen::Vector3d current_angular_velocity = imu_data.angular_velocity;
            Eigen::Vector3d current_acceleration = imu_data.acceleration;

            mean_angular_velocity += (current_angular_velocity - mean_angular_velocity) / (i + 1);
            mean_acceleration += (current_acceleration - mean_acceleration) / (i + 1);

            Eigen::Vector3d angular_velocity_covariance = Eigen::Vector3d::Zero();
            Eigen::Vector3d acceleration_covariance = Eigen::Vector3d::Zero();

            angular_velocity_covariance =
                angular_velocity_covariance * i / (i + 1.0) + (current_angular_velocity - mean_angular_velocity)
                                                                      .cwiseProduct(current_angular_velocity - mean_angular_velocity) *
                                                                  i / ((i + 1.0) * (i + 1.0));
            acceleration_covariance =
                acceleration_covariance * i / (i + 1.0) + (current_acceleration - mean_acceleration)
                                                                  .cwiseProduct(current_acceleration - mean_acceleration) *
                                                              i / ((i + 1.0) * (i + 1.0));
            mutex_.lock();
            if(!imu_data_buffer_.empty())
                imu_data_buffer_.pop();
            mutex_.unlock();
        }

        /* normalizing the acceleration to gravity*/
        acceleration_normalizer_ = GRAVITY / mean_acceleration.norm();

        /* esekf initialization */
        std::vector<double> epsi(23, 0.001);
        kf_.init_dyn_share(
            get_f,
            df_dx,
            df_dw,
            observation_function_,
            ikfom_max_iterations_,
            epsi.data());

        /* get state */
        state_ikfom init_state = kf_.get_x();
        /* init position and orientation */
        init_state.pos = initial_transform.topRightCorner(3, 1);
        Eigen::Quaterniond q(initial_transform.topLeftCorner<3, 3>());
        init_state.rot = q;
        /* gravity initialized by using measured mean gravity */
        init_state.grav = S2(-mean_acceleration * acceleration_normalizer_);
        /* gyro initialized by mean angular velocity */
        init_state.bg = mean_angular_velocity;
        /* initial tranform */
        init_state.offset_T_L_I = initial_lidar_translation_wrt_imu_;
        init_state.offset_R_L_I = initial_lidar_rotation_wrt_imu_;
        /* apply the initial state */
        kf_.change_x(init_state);

        /* get covariance */
        esekfom::esekf<state_ikfom, 12, input_ikfom>::cov init_covariance = kf_.get_P();
        init_covariance.setIdentity();

        /* TODO: Modification needed */
        init_covariance(6, 6) = init_covariance(7, 7) = init_covariance(8, 8) = 0.00001;
        init_covariance(9, 9) = init_covariance(10, 10) = init_covariance(11, 11) = 0.00001;
        init_covariance(15, 15) = init_covariance(16, 16) = init_covariance(17, 17) = 0.0001;
        init_covariance(18, 18) = init_covariance(19, 19) = init_covariance(20, 20) = 0.001;
        init_covariance(21, 21) = init_covariance(22, 22) = 0.00001;
        /* apply the initial covariance */
        kf_.change_P(init_covariance);

        // ----raistlin add
        kf2_.init_dyn_share(
            get_f,
            df_dx,
            df_dw,
            observation_function_,
            ikfom_max_iterations_,
            epsi.data());
        kf2_.change_x(init_state);
        kf2_.change_P(init_covariance);
        // ----raistlin add

        /* current state */
        state_time_ = imu_data.time;
        state_input_.acc = imu_data.acceleration;
        state_input_.gyro = imu_data.angular_velocity;
    }

    /** push IMU data to buffer
     *
     * @param t IMU timestamp
     * @param angular_velocity angular velocity measured from IMU
     * @param acceleration acceleration measured from IMU
     * @return None
     *
     */
    void Lio::PushImuData(double t, Eigen::Vector3d angular_velocity, Eigen::Vector3d acceleration)
    {
        imu_data_t imu_data;
        imu_data.time = t;
        imu_data.angular_velocity = angular_velocity;
        imu_data.acceleration = acceleration;

        std::lock_guard<std::mutex> lock(mutex_);

        imu_data_buffer_.push(imu_data);
        // std::cout << "imu_data_buffer: " << imu_data_buffer_.size() << std::endl;

        // ----raistlin add
        imu_data_buffer2_.push_back(imu_data);
        // std::cout << "imu_data_buffer2: " << imu_data_buffer2_.size() << std::endl;
        // ----raistlin add
    }

    /** push pointcloud data to buffer after preprocessing
     *
     * @param t IMU timestamp
     * @param pointcloud pointcloud pointer
     * @return None
     *
     */
    void Lio::PushPointcloud(double t, const PointCloudType::Ptr pointcloud)
    {
        lidar_data_t lidar_data;
        lidar_data.time = t;
        lidar_data.pointcloud = pointcloud;

        std::lock_guard<std::mutex> lock(mutex_);

        /* remove points outside the range limits */
        RemoveOutlierPoints_(pointcloud);

        lidar_data_buffer_.push(lidar_data);
        // std::cout << "lidar_data_buffer: " << lidar_data_buffer_.size() << std::endl;
    }

    /** propagate forward, calculate states at all IMU data time
     *
     * @param state_list store states
     * @param last_point_time the last point time in the current lidar frame
     * @return None
     *
     */
    void Lio::PropagateForward_(std::vector<std::tuple<double, input_ikfom, state_ikfom>> &state_list, double last_point_time)
    {
        state_list.push_back(std::tuple<double, input_ikfom, state_ikfom>(state_time_, state_input_, kf_.get_x()));

        imu_data_t imu_data_prev = imu_data_buffer_.front();
        mutex_.lock();
        if(!imu_data_buffer_.empty())
            imu_data_buffer_.pop();
        mutex_.unlock();
        /* state forward propagation for every IMU data */
        while (last_point_time > imu_data_buffer_.front().time)
        {
            imu_data_t imu_data = imu_data_buffer_.front();

            double dt = imu_data.time - state_time_;

            /* use average value is better */
            input_ikfom u;
            u.acc = imu_data.acceleration * acceleration_normalizer_;
            u.gyro = imu_data.angular_velocity;

            kf_.predict(dt, noise_covariance, u);

            state_time_ = imu_data.time;

            state_input_ = u;

            /* record states updated by IMU data*/
            state_list.push_back(std::tuple<double, input_ikfom, state_ikfom>(state_time_, u, kf_.get_x()));
        mutex_.lock();
            if(!imu_data_buffer_.empty())
                imu_data_buffer_.pop();
        mutex_.unlock();
            imu_data_prev = imu_data;
        }

        /* update the state for the last point of this scan */
        input_ikfom u;
        u.acc = imu_data_buffer_.front().acceleration * acceleration_normalizer_;
        u.gyro = imu_data_buffer_.front().angular_velocity;
        double dt = last_point_time - state_time_;
        kf_.predict(dt, noise_covariance, u);
        state_time_ = last_point_time;
        state_list.push_back(std::tuple<double, input_ikfom, state_ikfom>(state_time_, u, kf_.get_x()));
    }
    //--------DJCadd
    bool Lio::CheckIMU_(std::vector<std::tuple<double, input_ikfom, state_ikfom>> &state_list)
    {
        bool tag = true;
        int num = 0;
        double acc_x = 0;
        double acc_x_mean = 0;
        double acc_x_total = 0;
        double acc_x_D = 0;
        double acc_y = 0;
        double acc_y_mean = 0;
        double acc_y_total = 0;
        double acc_y_D = 0;
        double acc_z = 0;
        double acc_z_mean = 0;
        double acc_z_total = 0;
        double acc_z_D = 0;
        for (auto state = state_list.end() - 1; state != state_list.begin(); state--)
        {
            Eigen::Vector3d acceleration{std::get<1>(*state).acc};
            acc_x_total += acceleration.x();
            acc_y_total += acceleration.y();
            acc_z_total += acceleration.z();
            num++;
        }
        acc_x_mean = acc_x_total / num;
        acc_y_mean = acc_y_total / num;
        acc_z_mean = acc_z_total / num;
        for (auto state = state_list.end() - 1; state != state_list.begin(); state--)
        {
            Eigen::Vector3d acceleration{std::get<1>(*state).acc};
            acc_x_D += (acceleration.x() - acc_x_mean) * (acceleration.x() - acc_x_mean);
            acc_y_D += (acceleration.y() - acc_y_mean) * (acceleration.y() - acc_y_mean);
            acc_z_D += (acceleration.z() - acc_z_mean) * (acceleration.z() - acc_z_mean);
        }
        acc_x_D = sqrt(acc_x_D) / num;
        acc_y_D = sqrt(acc_y_D) / num;
        acc_z_D = sqrt(acc_z_D) / num;
#ifdef LIO_DEBUG
        std::cout << "acc_x_D:" << acc_x_D << std::endl;
        std::cout << "acc_y_D:" << acc_y_D << std::endl;
        std::cout << "acc_z_D:" << acc_z_D << std::endl;
#endif // LIO_DEBUG
        if (acc_x_D > 0.4)
        {
#ifdef LIO_DEBUG
            std::cout << "acc_x_D>0.4! stop mapping, now acc_x_D:" << acc_x_D << std::endl;
#endif // LIO_DEBUG
            tag = false;
        }
        if (acc_y_D > 0.4)
        {
#ifdef LIO_DEBUG
            std::cout << "acc_y_D>0.4! stop mapping, now acc_y_D:" << acc_y_D << std::endl;
#endif // LIO_DEBUG
            tag = false;
        }
        if (acc_z_D > 0.4)
        {
#ifdef LIO_DEBUG
            std::cout << "acc_z_D>0.4! stop mapping, now acc_z_D:" << acc_z_D << std::endl;
#endif // LIO_DEBUG
            tag = false;
        }
        return tag;
    }
    /** propagate backward, undistort all points
     *
     * @param state_list store states
     * @param lidar_data the current lidar frame
     * @return None
     *
     */
    void Lio::UndistortPointCloud_(std::vector<std::tuple<double, input_ikfom, state_ikfom>> &state_list, lidar_data_t &lidar_data)
    {
        auto &points = lidar_data.pointcloud->points;
        auto frame_end_state = std::get<2>(*(state_list.end() - 1));
        std::vector<int> point_num(state_list.size() + 1);

        point_num[0] = points.size();
        // #   pragma omp paralell for
        for (auto state = state_list.end() - 1; state != state_list.begin(); state--)
        {
            auto p_it = std::find_if(points.begin(), points.end(), [&](PointType p)
                                     { return p.t * 1e-9 + lidar_data.time < std::get<0>(*state); });
            point_num[state - state_list.begin()] = p_it - points.begin();
        }
        *(point_num.end() - 1) = 0;
        std::reverse(point_num.begin(), point_num.end());

        for (auto state = state_list.end() - 1; state != state_list.begin(); state--)
        {
            Eigen::Vector3d position{std::get<2>(*state).pos};
            Eigen::Vector3d velocity{std::get<2>(*state).vel};
            Eigen::Matrix3d rotation{std::get<2>(*state).rot.toRotationMatrix()};
            Eigen::Vector3d acceleration{std::get<1>(*state).acc};
            Eigen::Vector3d angular_velocity{std::get<1>(*state).gyro};

            // #       pragma omp paralell for
            for (auto index = point_num[state_list.end() - 1 - state];
                 index < point_num[state_list.end() - state];
                 index++)
            {
                double dt = points[index].t * 1e-9 + lidar_data.time - std::get<0>(*state);

                Eigen::Matrix3d rotation_interpolation{rotation * Exp(angular_velocity, dt)};
                Eigen::Vector3d translation_interpolation{position + velocity * dt + 0.5 * acceleration * dt * dt - std::get<2>(*state).pos};
                Eigen::Vector3d position_of_point{points[index].x, points[index].y, points[index].z};
                Eigen::Vector3d point_compensated{frame_end_state.offset_R_L_I.conjugate() * (frame_end_state.rot.conjugate() * (rotation_interpolation * (std::get<2>(*state).offset_R_L_I * position_of_point + std::get<2>(*state).offset_T_L_I) + translation_interpolation) - frame_end_state.offset_T_L_I)};
                points[index].x = point_compensated.x();
                points[index].y = point_compensated.y();
                points[index].z = point_compensated.z();
            }
        }
    }

    /** mapping
     *
     * @param void
     * @return None
     *
     */
    void Lio::Mapping()
    {
        std::vector<PointType, Eigen::aligned_allocator<PointType>> points;
        float k = -2.36075470841859;
        float b = 50.5311325972969;
        for (auto iter = points_add_to_map_.begin(); iter != points_add_to_map_.end(); iter++)
        {
            if (current_lidar_data_.pointcloud->points[*iter].z < -5) // 地面以下
            {
                continue;
            }
            if (current_lidar_data_.pointcloud->points[*iter].z > 4.5) // 房顶
            {
                continue;
            }
            if (current_lidar_data_.pointcloud->points[*iter].y > -4.3 && current_lidar_data_.pointcloud->points[*iter].y < 20) // 起点
            {
                continue;
            }
            if (k * current_lidar_data_.pointcloud->points[*iter].x + b - current_lidar_data_.pointcloud->points[*iter].y < 5) // 上料口区域
            {
                continue;
            }
            if (current_lidar_data_.pointcloud->points[*iter].x > -26.7 && current_lidar_data_.pointcloud->points[*iter].x < 11.6 && current_lidar_data_.pointcloud->points[*iter].y > -14.9 && current_lidar_data_.pointcloud->points[*iter].y < -13.9) // 第二个墙
            {
                continue;
            }
            if (current_lidar_data_.pointcloud->points[*iter].x > -31.5 && current_lidar_data_.pointcloud->points[*iter].x < 16.6 && current_lidar_data_.pointcloud->points[*iter].y > -26.1 && current_lidar_data_.pointcloud->points[*iter].y < -24.7) // 第三个墙
            {
                continue;
            }
            if (current_lidar_data_.pointcloud->points[*iter].x > -36.3 && current_lidar_data_.pointcloud->points[*iter].x < 21 && current_lidar_data_.pointcloud->points[*iter].y > -36.9 && current_lidar_data_.pointcloud->points[*iter].y < -35.9) // 第四个墙
            {
                continue;
            }
            if (current_lidar_data_.pointcloud->points[*iter].x > -41.5 && current_lidar_data_.pointcloud->points[*iter].x < 25.8 && current_lidar_data_.pointcloud->points[*iter].y > -48.1 && current_lidar_data_.pointcloud->points[*iter].y < -46.9) // 第五个墙
            {
                continue;
            }
            if (current_lidar_data_.pointcloud->points[*iter].x > -41.5 && current_lidar_data_.pointcloud->points[*iter].x < 30.4 && current_lidar_data_.pointcloud->points[*iter].y > -58.9 && current_lidar_data_.pointcloud->points[*iter].y < -57.9) // 第六个墙
            {
                continue;
            }
            if (current_lidar_data_.pointcloud->points[*iter].x > -41.5 && current_lidar_data_.pointcloud->points[*iter].x < 35 && current_lidar_data_.pointcloud->points[*iter].y > -69.9 && current_lidar_data_.pointcloud->points[*iter].y < -68.7) // 第七个墙
            {
                continue;
            }
            if (current_lidar_data_.pointcloud->points[*iter].x > -41.5 && current_lidar_data_.pointcloud->points[*iter].x < 44.6 && current_lidar_data_.pointcloud->points[*iter].y > -91.9 && current_lidar_data_.pointcloud->points[*iter].y < -90.7) // 第八个墙
            {
                continue;
            }
            points.push_back(current_lidar_data_.pointcloud->points[*iter]);
        }
#ifdef LIO_DEBUG
        std::cout << "Points add to map: " << points_add_to_map_.size() << "\n";
#endif // LIO_DEBUG

        ivox_->Mapping_AddPoints(points);
        // slam::PointCloudType::Ptr test_map(new slam::PointCloudType);
        // test_map = ivox_->GetPointCloud();
        // printf("now map total size=%d", test_map->size());
    }

    /** main process
     *
     * @param PointCloudType::Ptr store states
     * @param Eigen::Vector3d the current translation
     * @param Eigen::Quaterniond the current rotation
     * @param Eigen::Vector3d the current velocity
     * @return 0 for nothing happened, 1 for initialized, 2 for slam iterated, -1 for error detected
     *
     */
    int Lio::Run(PointCloudType::Ptr &pointcloud, Eigen::Vector3d &translation, Eigen::Quaterniond &rotation, Eigen::Vector3d &velocity)
    {

        if (imu_data_buffer_.empty())
        {
            return 0;
        }

        if (lio_state_ == lio_state_t::INITIALIZING)
        {
            int imu_data_buffer_size = imu_data_buffer_.size();
            // no enough IMU data accumulated
            if (imu_data_buffer_size < minimum_initial_imu_data_)
            {
                return 0;
            }
#ifdef LIO_DEBUG
            std::cout << "********** ********** ********** ********** ********** **********\n";
            std::cout << "INITIALIZING, lidar buffer: " << lidar_data_buffer_.size() << ", imu buffer: " << imu_data_buffer_.size() << std::endl;
#endif // LIO_DEBUG

            // enough imu data sampled
            InitializeKF_();

#ifdef LIO_DEBUG
            std::cout << "IKFoM Initialized." << std::endl;
#endif // LIO_DEBUG

            // clear lidar data buffer
            lidar_data_buffer_ = std::queue<lidar_data_t>();

            // set lio state to RUNNING
            lio_state_ = lio_state_t::RUNNING;
            return 1;
        }
        else // lio_state_ == lio_state_t::RUNNING
        {
            if (!lidar_data_buffer_.empty())
            {
#ifdef LIO_DEBUG
                auto t_start = std::chrono::system_clock::now();
#endif // LIO_DEBUG

#ifdef LIO_DEBUG
                std::cout << "---------- ---------- ---------- ---------- ---------- ----------\n";
                std::cout << "RUNNING, lidar buffer: " << lidar_data_buffer_.size() << ", imu buffer: " << imu_data_buffer_.size() << std::endl;
                double d_start = std::chrono::duration_cast<std::chrono::milliseconds>(t_start.time_since_epoch()).count() / 1000.0;
                std::cout << lidar_data_buffer_.front().time << ",  " << d_start << std::endl;
#endif // LIO_DEBUG

                current_lidar_data_ = lidar_data_buffer_.front();

                auto &points = current_lidar_data_.pointcloud->points;

                // sort points by their timestamps in inverse order
                std::sort(
                    points.begin(),
                    points.end(),
                    [](PointOuster &p1, PointOuster &p2)
                    { return p1.t > p2.t; });

                double last_point_time = points.begin()->t * 1e-9 + current_lidar_data_.time;

#ifdef LIO_DEBUG
                auto t_sort = std::chrono::system_clock::now();
                std::cout << "    Sort: " << std::chrono::duration_cast<std::chrono::microseconds>(t_sort - t_start).count() << " us,\n";
#endif // LIO_DEBUG

                // the IMU data later than the lidar data is not arrived
                if (last_point_time > imu_data_buffer_.back().time)
                {
#ifdef LIO_DEBUG
                    std::cout << "****the IMU data later than the lidar data is not arrived" << std::endl;
#endif // LIO_DEBUG
                    return 0;
                }

                pcl::VoxelGrid<PointType> voxel_filter;
                voxel_filter.setLeafSize(downsampling_size_, downsampling_size_, downsampling_size_);
                voxel_filter.setInputCloud(current_lidar_data_.pointcloud);
                voxel_filter.filter(*current_lidar_data_.pointcloud);

                // too few points
                if (current_lidar_data_.pointcloud->size() < 5)
                {
#ifdef LIO_DEBUG
                    std::cout << "****too few points" << std::endl;
#endif // LIO_DEBUG
        mutex_.lock();
                    if(!lidar_data_buffer_.empty())
                        lidar_data_buffer_.pop();
        mutex_.unlock();
                    return 0;
                }

#ifdef LIO_DEBUG
                auto t_downsampling = std::chrono::system_clock::now();
                std::cout << "    Downsampling: " << std::chrono::duration_cast<std::chrono::microseconds>(t_downsampling - t_sort).count() << " us,\n";
#endif // LIO_DEBUG

                // record every state from the last point of the last scan
                std::vector<std::tuple<double, input_ikfom, state_ikfom>> state_list;
                PropagateForward_(state_list, last_point_time);
                bool if_mapping = false;
                if(!state_list.empty())
                    if_mapping = CheckIMU_(state_list);

#ifdef LIO_DEBUG
                auto t_undistort_state_estimate = std::chrono::system_clock::now();
                std::cout << "    State Estimatie: " << std::chrono::duration_cast<std::chrono::microseconds>(t_undistort_state_estimate - t_downsampling).count() << " us,\n";
#endif // LIO_DEBUG

                UndistortPointCloud_(state_list, current_lidar_data_);

#ifdef LIO_DEBUG
                auto t_undistort = std::chrono::system_clock::now();
                std::cout << "    Undistort: " << std::chrono::duration_cast<std::chrono::microseconds>(t_undistort - t_start).count() << " us,\n";
#endif // LIO_DEBUG
        mutex_.lock();
                if(!lidar_data_buffer_.empty())
                    lidar_data_buffer_.pop();
        mutex_.unlock();

                // no point in the map
                if (0 == ivox_->NumValidGrids())
                {
#ifdef LIO_DEBUG
                    std::cout << "****add initial pointcloud" << std::endl;
#endif // LIO_DEBUG
                    ivox_->Mapping_AddPoints(current_lidar_data_.pointcloud->points);
                    return 0;
                }

                // residual calculation
                double solve_time;
                kf_.update_iterated_dyn_share_modified(lidar_covariance_, solve_time);

                state_ikfom state = kf_.get_x();
                esekfom::esekf<state_ikfom, 12, input_ikfom>::cov covariance = kf_.get_P();

                // ----raistlin add
                mutex_.lock();
                kf2_.change_x(state);
                kf2_.change_P(covariance);
                lidar_time_ = last_point_time;
                imu_time_ = last_point_time;
                lidar_updated_ = 15;
                mutex_.unlock();

                bool validity = CheckValidity_(state);
                if (!validity)
                {
#ifdef LIO_DEBUG
                    std::cout << "****current cloud doesn't match the map" << std::endl;
#endif // LIO_DEBUG
                    return -1;
                }
                // ----raistlin add

#ifdef LIO_DEBUG
                auto t_residual = std::chrono::system_clock::now();
                std::cout << "    Residual: " << std::chrono::duration_cast<std::chrono::microseconds>(t_residual - t_undistort).count() << " us,\n";
#endif // LIO_DEBUG

                // #           pragma omp parallel for
                for (auto point_iterator = current_lidar_data_.pointcloud->points.begin();
                     point_iterator < current_lidar_data_.pointcloud->points.end();
                     point_iterator++)
                {
                    point_iterator->getVector3fMap() =
                        state.rot.cast<float>() *
                            (state.offset_R_L_I.cast<float>() * point_iterator->getVector3fMap() + state.offset_T_L_I.cast<float>()) +
                        state.pos.cast<float>();
                    point_iterator->range = 0;
                }
                if (valid_rate > 0.4 && if_mapping)
                {
#ifdef LIO_DEBUG
                    std::cout << "    valid rate >0.4 and acc small, Mapping! " << std::endl;
#endif // LIO_DEBUG
                    Mapping();
                }
                else
                {
#ifdef LIO_DEBUG
                    std::cout << "    valid rate <0.4 or acc TOO BIG,stop Mapping! " << std::endl;
#endif // LIO_DEBUG
                }
                points_add_to_map_.clear();
#ifdef LIO_DEBUG
                std::cout << "now vel = " << state.vel.norm() << std::endl;
#endif // LIO_DEBUG

#ifdef LIO_DEBUG
                auto t_mapping = std::chrono::system_clock::now();
                std::cout << "    Mapping: " << std::chrono::duration_cast<std::chrono::microseconds>(t_mapping - t_residual).count() << " us." << std::endl;
#endif // LIO_DEBUG

                pointcloud = current_lidar_data_.pointcloud;
                translation = state.pos;                                     // Eigen::Vector3d
                rotation = Eigen::Quaterniond(state.rot.toRotationMatrix()); // Eigen::Matrix3d = state.rot.toRotationMatrix()
                velocity = state.vel;                                        // Eigen::Vector3d
                millisleep(30);

#ifdef LIO_DEBUG
                int time = std::chrono::duration_cast<std::chrono::microseconds>(t_mapping - t_start).count();
                avg_time_ = (avg_time_ * frames_ + time) / (frames_ + 1);
                max_time_ = max_time_ > time ? max_time_ : time;
                frames_++;

                std::cout << "time: " << time / 1000 << " ms.\n";
                std::cout << "max time: " << max_time_ / 1000 << " ms.\n";
                std::cout << "average time: " << avg_time_ / 1000 << " ms in " << frames_ << " frames." << std::endl;
#endif // LIO_DEBUG

                return 2;
            }

            return 0;
        }
    }

    /** generate results process
     *
     * @param double the current imu time
     * @param Eigen::Vector3d the current position
     * @param Eigen::Quaterniond the current rotation
     * @param Eigen::Vector3d the current velocity
     * @return true for running, false for pausing
     *
     */
    bool Lio::GenerateResults(double &d_time, Eigen::Vector3d &v_pos, Eigen::Quaterniond &q_rot, Eigen::Vector3d &v_vel)
    {
        // limit iterations by lidar_updated_
        if (lio_state_ != lio_state_t::RUNNING || lidar_updated_ <= 0)
        {
            return false;
        }
        --lidar_updated_;

        // update {imu_data_buffer2_, imu_data_iter2_(iterator), imu_data_i2_(int index)} if lidar_time_ updated
        mutex_.lock();
        while (!imu_data_buffer2_.empty())
        {
            if ((*imu_data_buffer2_.begin()).time <= lidar_time_)
            {
                imu_data_buffer2_.pop_front();
                imu_data_i2_ = 0;
                imu_data_iter2_ = imu_data_buffer2_.begin();
            }
            else
            {
                break;
            }
        }
        mutex_.unlock();

        // if lidar_time_ is updated, imu_data_iter2_ = imu_data_buffer2_.begin()
        // if lidar_time_ is not updated, imu_data_iter2_ = imu_data_iter2_pre_ + 1
        if (imu_data_iter2_ != imu_data_buffer2_.begin())
        {
            imu_data_iter2_ = imu_data_iter2_pre_;
            ++imu_data_iter2_;
        }

        // update kf2_ for count times or to the end of imu_data_buffer2_
        imu_data_t imu_data;
        input_ikfom u;
        double dt;
        size_t count = 100; // imu_data_buffer2_.size();
        while (imu_data_iter2_ != imu_data_buffer2_.end() && count > 0)
        {
            // std::cout << count << ", " << imu_data_i2_ << " / " << imu_data_buffer2_.size() << std::endl;
            --count;
            imu_data_iter2_pre_ = imu_data_iter2_;

            imu_data = *imu_data_iter2_;
            if (imu_data.time <= lidar_time_)
                break;
            u.acc = imu_data.acceleration * acceleration_normalizer_;
            u.gyro = imu_data.angular_velocity;

            ++imu_data_i2_;
            ++imu_data_iter2_;

            mutex_.lock();
            dt = imu_data.time - imu_time_;
            kf2_.predict(dt, noise_covariance, u);
            imu_time_ = imu_data.time;
            mutex_.unlock();
        }

        state_ikfom state = kf2_.get_x();
        d_time = imu_time_;
        v_pos = state.pos;
        q_rot = Eigen::Quaterniond(state.rot.toRotationMatrix());
        v_vel = state.vel;

        return true;
    }

    /** check buffers
     *
     * @param size_t lidar_data_buffer_ size
     * @param size_t imu_data_buffer_ size
     * @param size_t imu_data_buffer2_ size
     * @return None
     *
     */
    void Lio::CheckBuffers(size_t &lidar_size, size_t &imu_size, size_t &imu2_size, size_t &imu2_i)
    {
        lidar_size = lidar_data_buffer_.size();
        imu_size = imu_data_buffer_.size();
        imu2_size = imu_data_buffer2_.size();
        imu2_i = imu_data_i2_;
    }

    /** check buffers
     *
     * @param size_t lidar_data_buffer_ size
     * @param size_t imu_data_buffer_ size
     * @param size_t imu_data_buffer2_ size
     * @param double the first lidar_data_buffer_ time
     * @param double the first imu_data_buffer_ time
     * @param double the first imu_data_buffer2_ time
     * @return None
     *
     */
    void Lio::CheckBuffers(size_t &lidar_size, size_t &imu_size, size_t &imu2_size, size_t &imu2_i, double &lidar_time, double &imu_time, double &imu2_time)
    {
        mutex_.lock();

        lidar_size = lidar_data_buffer_.size();
        if (lidar_size > 0)
            lidar_time = lidar_data_buffer_.front().time;
        else
            lidar_time = 0.0;

        imu_size = imu_data_buffer_.size();
        if (imu_size > 0)
            imu_time = imu_data_buffer_.front().time;
        else
            imu_time = 0.0;

        imu2_size = imu_data_buffer2_.size();
        imu2_i = imu_data_i2_;
        if (imu_data_iter2_ != imu_data_buffer2_.end())
            imu2_time = (*imu_data_iter2_).time;
        else
            imu2_time = 0.0;

        mutex_.unlock();
    }
    void Lio::millisleep(unsigned int milliseconds) {
    struct timespec ts;
    // 将毫秒转换为秒和纳秒
    ts.tv_sec = milliseconds / 1000;
    ts.tv_nsec = (milliseconds % 1000) * 1000000;
 
    // 使用nanosleep进行休眠
    while ((nanosleep(&ts, &ts) == -1) && (errno == EINTR));
}

} // namespace slam
